#include "Arduino.h"
#include "Xbee.h"
#include "definitions.h"
#include <Wire.h>

//This will assume that serial has begun at some point previously
Xbee::Xbee(){

}

Xbee::Xbee(NetworkListener* listener){
	_listener = listener;
}

void Xbee::prepare(){
	_resetPin = RESETTRANSISTORPIN_DIGITAL_OUT_PIN;
	pinMode(_resetPin, OUTPUT);
	//digitalWrite(_resetPin, LOW);
	_myAdress = XX0;
	_qbId = 0;
	//reset serial begin to get them properly set
	//TODO change baudrate down
	_myAdress = getXbeeModemATSL();
	joinQbNetwork(); 
	delay(1000); //This delay is to get out of AT mode
}

String Xbee::getXbeeAdress(){
	return _myAdress;
}

void Xbee::setQbId(char id){
	_qbId = id;	
}

char Xbee::getQbId(){
	return _qbId;
}

String Xbee::getXbeeModemATSL(){
  	//These serialz remains
  	Serial.flush(); //Clean upoz0r?
 	// Serial.println("Getting Serial Number low, be patient");
  	delay(1100);      //Delay required before trying to enter command mode
  	Serial.print("+++"); //OK
  	delay(1100);

  	Serial.print("ATSL\r"); //Fetch internal adress //OK
  	delay(5);
  	Serial.print("ATCN\r"); //Return to normal mode //OK
  	delay(200);      
        
  	String addr = "";
	long start = millis();
	while (addr.length() < 8 && millis() - start < 1000) {
		while(Serial.available() > 0) {
			char inByte = Serial.read();
			if(inByte != '\r' && inByte != 'O' && inByte != 'K'){
				addr +=  inByte;
			}
		}
	}
	return addr;
}

void Xbee::joinResetNetwork(){ 
  //Remember to have a COORDINATOR FFS
  //ATID1939,DH0013A200,DL406FC95D,BD7,RO10,WR,CN
  Serial.flush(); //Clean upoz0r?
 // Serial.println("Joining ResetNetwork");
  delay(1100);      //Delay required before trying to enter command mode
  Serial.print("+++");
  delay(1100);
  Serial.print("ATID1939,DH13A200,DL407A393F,BD7,RO10,WR,CN\r"); 
  delay(1100);
}


void Xbee::resetNetworking(){ 
  Serial.flush(); //Clean upoz0r?
 // Serial.println("Joining ResetNetwork");
  delay(1100);      //Delay required before trying to enter command mode
  Serial.print("+++");
  delay(1100);
  Serial.print("ATNR0,CN\r"); //ATNR0 resets this device, ATNR1 resets all devices
  delay(1100);
}

void Xbee::joinQbNetwork(){
  //ATID1939,DH0013A200,DL406FC95D,BD7,RO10,WR,CN
  Serial.flush(); //Clean upoz0r?
 // Serial.println("Joining Qbnetwork");
  delay(1100);      //Delay required before trying to enter command mode
  Serial.print("+++");
  delay(1100);
  Serial.print("ATID1337,DH0,DLFFFF,BD7,RO10,WR,CN\r"); //ATcommands for this
  delay(1100);
}


void Xbee::update(){
	updateSerial();
//	updateRoom();
}

void Xbee::updateRoom(){

}


void Xbee::updateSerial() {  
  if (Serial.available() == 0 || !properMessageStart()) { return; }
  
  int  read = 1;  
  char command = '*';
  char data[3];
  long start = millis();
	while (read < 5) {
    if (millis() - start > SERIAL_WAIT) { return; } // Vi har brukt for lang tid -> avbryt
    if (Serial.available() == 0) { continue; }
    char c = Serial.read();
    if (c == '_' || c == ' ') { return; } // Ugyldige verdier, returner
    read++;
    if (read == 2) { command = c; }
    else           { data[read - 3] = c; }
	}
  dispatchCommand(command, data);
}

boolean Xbee::properMessageStart() {
  while (Serial.available() > 0) {
    char in = Serial.read();
    if (in == '_') { return true; }
  }
  return false;
}

void Xbee::dispatchCommand(char command, char* data) {
  switch(command) {
    case 'r': processResetCommand(data); break;
    case 's': processStateCommands(data); break;
    case 'b': processBroadCastCommands(data); break;
    case 'd': processDebugCommands(data); break;
    case 'n': processNetworkCommands(data); break;
	}
}

void Xbee::processResetCommand(char* data){ //_r@88 for default //_r088 for alle
	char qbId = data[0];
  int command = getCent(data);
  
  if (qbId == '0' || _listener->getQbId() == qbId) {
	   switch (command) {
        case 99: joinResetNetwork(); break;
        case 88: reset(1000); break; // Simple reset
        case 66: reset(3000); break; // Reset for upload or debug
  	 }
	}	
}

void Xbee::processBroadCastCommands(char* data){
  char qbId = data[0];
  int command = getCent(data);
  
	if (qbId == '0' || _listener->getQbId() == qbId) {
    _listener->commandReceived(command);
	}
}

void Xbee::processStateCommands(char* data){
	_listener->stateChanged(data[0], data[1], data[2]);
}


void Xbee::processDebugCommands(char* data) {
	char qbId = data[0];
  int command = getCent(data);
  
	if (qbId == '0' || _listener->getQbId() == qbId) {
	  switch (command) {
      case 1: Serial.print(SWVERSION);Serial.print("-");Serial.println(_qbId); break;
	  }
	}
}

void Xbee::processNetworkCommands(char* data){
	char qbId = data[0];
  int command = getCent(data);
  
	if (qbId == '0' || _listener->getQbId() == qbId) {
	   switch (command) {
        case 1:  joinQbNetwork(); break;
        case 99: joinResetNetwork(); break;
  	 }
	}
}

int Xbee::getCent(char* data) {
  char buffer[3];
  buffer[0] = data[1];
	buffer[1] = data[2];
	buffer[2] = '\0';
  return atoi(buffer);
}

void Xbee::ping(){

}

void Xbee::broadcastCommand(char id, char data_1, char data_2){	
	Serial.print("_"); Serial.print("b"); Serial.print(id);Serial.print(data_1);Serial.print(data_2);
	Serial.println(); //This is an LN to get carriage return
}

void Xbee::broadcastCommand(char id, int data){	
	Serial.print("_"); Serial.print("b"); Serial.print(id);
	if (data < 10) {
    Serial.print('0');
	}
	Serial.print(data);
	Serial.println(); //This is an LN to get carriage return
}

void Xbee::broadcastState(int state, int value){	
	Serial.print("_"); Serial.print("s"); Serial.print(_qbId); Serial.print(state); Serial.print(value);
	Serial.println(); //This is an LN to get carriage return
}

void Xbee::reset(int delayTime){
//	Serial.print("Resetting cube: ");
//	Serial.println(_qbId);
	delay(delayTime); //This needs to be synced with the lovely upload.sh script to make any sense
	
	digitalWrite(_resetPin, HIGH); 
}

